#pragma once

#include <math.h>

OPEN_JLIB_NS


struct Vec2 {

  f64 p;

  inline Vec2 () noexcept {
  }
  inline Vec2 (Vec2i v) noexcept {
    asm volatile (R"(
    scvtf %[dst].2s, %[src].2s)"
    : [dst]"=x"(p): [src]"x"(v.p));
  }
  inline Vec2 (f64 p) noexcept : p(p) {
  }
  inline Vec2 (f32 x, f32 y) noexcept {
    asm volatile (R"(
    zip1 %[dst].2s, %[op1].2s, %[op2].2s)"
    : [dst]"=x"(p): [op1]"x"(x), [op2]"x"(y));
  }
  inline Vec2 (f32 s) noexcept {
    asm volatile (R"(
    dup %[dst].2s, %[src].s[0])"
    : [dst]"=x"(p): [src]"x"(s));
  }
  inline Vec2 (const f32* m) noexcept {
    asm volatile (R"(
    ld1 { %[dst].2s }, [%[mem]])"
    : [dst]"=x"(p): [mem]"r"(m));
  }


  inline f32 x () const noexcept {
    f32 r;
    asm volatile (R"()"
    : [dst]"=r"(r): "[dst]"(p));
    return r;
  }
  inline f32 y () const noexcept {
    f32 r;
    asm volatile (R"(
    ins %[dst].s[0], %[src].s[1])"
    : [dst]"=r"(r): [src]"x"(p));
    return r;
  }


  inline Vec2& x (f32 s) noexcept {
    asm volatile (R"(
    ins %[dst].s[0], %[src].s[0])"
    : [dst]"+x"(p): [src]"x"(s));
    return *this;
  }
  inline Vec2& y (f32 s) noexcept {
    asm volatile (R"(
    ins %[dst].s[1], %[src].s[0])"
    : [dst]"+x"(p): [src]"x"(s));
    return *this;
  }


  inline Vec2& operator+= (Vec2 v) noexcept {
    asm volatile (R"(
    fadd %[dst].2s, %[dst].2s, %[src].2s)"
    : [dst]"+x"(p): [src]"x"(v.p));
    return *this;
  }
  inline Vec2& operator-= (Vec2 v) noexcept {
    asm volatile (R"(
    fsub %[dst].2s, %[dst].2s, %[src].2s)"
    : [dst]"+x"(p): [src]"x"(v.p));
    return *this;
  }
  inline Vec2& operator*= (Vec2 v) noexcept {
    asm volatile (R"(
    fmul %[dst].2s, %[dst].2s, %[src].2s)"
    : [dst]"+x"(p): [src]"x"(v.p));
    return *this;
  }
  inline Vec2& operator/= (Vec2 v) noexcept {
    asm volatile (R"(
    fdiv %[dst].2s, %[dst].2s, %[src].2s)"
    : [dst]"+x"(p): [src]"x"(v.p));
    return *this;
  }


  inline Vec2& operator*= (f32 s) noexcept {
    asm volatile (R"(
    fmul %[dst].2s, %[dst].2s, %[src].s[0])"
    : [dst]"+x"(p): [src]"x"(s));
    return *this;
  }
  inline Vec2& operator/= (f32 s) noexcept {
    f64 t;
    asm volatile (R"(
    dup %[tmp].2s, %[src].s[0]
    fdiv %[dst].2s, %[dst].2s, %[tmp].2s)"
    : [dst]"+x"(p), [tmp]"=x"(t): [src]"x"(s));
    return *this;
  }


  inline Vec2& clamp (Vec2 min, Vec2 max) noexcept {
    asm volatile (R"(
    fmin %[dst].2s, %[dst].2s, %[max].2s
    fmax %[dst].2s, %[dst].2s, %[min].2s)"
    : [dst]"+x"(p): [min]"x"(min.p), [max]"x"(max.p));
    return *this;
  }
  inline f32 normalize () noexcept {
    f32 r;
    asm volatile (R"(
    fmul %[dst].2s, %[src].2s, %[src].2s
    faddp %s[dst], %[dst].2s
    fsqrt %s[dst], %s[dst]
    dup %[dst].2s, %[dst].s[0]
    fdiv %[src].2s, %[src].2s, %[dst].2s)"
    : [dst]"=&x"(r), [src]"+x"(p));
    return r;
  }


  inline Vec2 operator+ (Vec2 v) const noexcept {
    f64 r;
    asm volatile (R"(
    fadd %[dst].2s, %[op1].2s, %[op2].2s)"
    : [dst]"=x"(r): [op1]"x"(p), [op2]"x"(v.p));
    return r;
  }
  inline Vec2 operator- (Vec2 v) const noexcept {
    f64 r;
    asm volatile (R"(
    fsub %[dst].2s, %[op1].2s, %[op2].2s)"
    : [dst]"=x"(r): [op1]"x"(p), [op2]"x"(v.p));
    return r;
  }
  inline Vec2 operator* (Vec2 v) const noexcept {
    f64 r;
    asm volatile (R"(
    fmul %[dst].2s, %[op1].2s, %[op2].2s)"
    : [dst]"=x"(r): [op1]"x"(p), [op2]"x"(v.p));
    return r;
  }
  inline Vec2 operator/ (Vec2 v) const noexcept {
    f64 r;
    asm volatile (R"(
    fdiv %[dst].2s, %[op1].2s, %[op2].2s)"
    : [dst]"=x"(r): [op1]"x"(p), [op2]"x"(v.p));
    return r;
  }


  inline Vec2 operator* (f32 s) const noexcept {
    f64 r, t;
    asm volatile (R"(
    fmul %[dst].2s, %[op1], %[op2].s[0])"
    : [dst]"=x"(r): [op1]"x"(p), [op2]"x"(s));
    return r;
  }
  inline Vec2 operator/ (f32 s) const noexcept {
    f64 r;
    asm volatile (R"(
    dup %[dst].2s, %[op2].s[0]
    fdiv %[dst].2s, %[op1].2s, %[dst].2s)"
    : [dst]"=&x"(r): [op1]"x"(p), [op2]"x"(s));
    return r;
  }


  inline f32 operator& (Vec2 v) const noexcept {
    f32 r;
    asm volatile (R"(
    fmul %[dst].2s, %[op1].2s, %[op2].2s
    faddp %s[dst], %[dst].2s)"
    : [dst]"=x"(r): [op1]"x"(p), [op2]"x"(v.p));
    return r;
  }
  inline f32 operator^ (Vec2 v) const noexcept {
    f32 r, t;
    asm volatile (R"(
    rev64 %[dst].2s, %[op2].2s
    fmul %[dst].2s, %[op1].2s, %[dst].2s
    ins %[tmp].s[0], %[dst].s[1]
    fsub %s[dst], %s[dst], %s[tmp])"
    : [dst]"=&x"(r), [tmp]"=x"(t): [op1]"x"(p), [op2]"x"(v.p));
    return r;
  }


  inline Vec2 operator+ () const noexcept {
    f64 r;
    asm volatile (R"(
    fabs %[dst].2s, %[src].2s)"
    : [dst]"=x"(r): [src]"x"(p));
    return r;
  }
  inline Vec2 operator- () const noexcept {
    f64 r;
    asm volatile (R"(
    fneg %[dst].2s, %[src].2s)"
    : [dst]"=x"(r): [src]"x"(p));
    return r;
  }
  inline Vec2 operator* () const noexcept {
    f64 r;
    asm volatile (R"(
    fmul %[dst].2s, %[src].2s, %[src].2s
    faddp %s[dst], %[dst].2s
    fsqrt %s[dst], %s[dst]
    dup %[dst].2s, %[dst].s[0]
    fdiv %[dst].2s, %[src].2s, %[dst].2s)"
    : [dst]"=&x"(r): [src]"x"(p));
    return r;
  }
  inline f32 operator~ () const noexcept {
    f32 r;
    asm volatile (R"(
    fmul %[dst].2s, %[src].2s, %[src].2s
    faddp %s[dst], %[dst].2s
    fsqrt %s[dst], %s[dst])"
    : [dst]"=x"(r): [src]"x"(p));
    return r;
  }


  inline static Vec2 zero () noexcept {
    f64 r;
    asm volatile (R"(
    eor %[dst].8b, %[dst].8b, %[dst].8b)"
    : [dst]"=x"(r));
    return r;
  }
  inline static Vec2 one () noexcept {
    return {1.0F, 1.0F};
  }
  template<u8 Index>
  inline static Vec2 unit () noexcept;


  inline static f32 distance (Vec2 a, Vec2 b) noexcept {
    return ~(b-a);
  }
  inline static Vec2 min (Vec2 a, Vec2 b) noexcept {
    f64 r;
    asm volatile (R"(
    fmin %[dst].2s, %[op1].2s, %[op2].2s)"
    : [dst]"=x"(r): [op1]"x"(a.p), [op2]"x"(b.p));
    return r;
  }
  inline static Vec2 max (Vec2 a, Vec2 b) noexcept {
    f64 r;
    asm volatile (R"(
    fmax %[dst].2s, %[op1].2s, %[op2].2s)"
    : [dst]"=x"(r): [op1]"x"(a.p), [op2]"x"(b.p));
    return r;
  }
  inline static Vec2 clamp (Vec2 v, Vec2 min, Vec2 max) noexcept {
    return Vec2::min(Vec2::max(v, min), max);
  }
  inline static Vec2 lerp (Vec2 a, Vec2 b, Vec2 t) noexcept {
    return a + (b-a)*t;
  }
  inline static Vec2 slerp (Vec2 a, Vec2 b, f32 t) noexcept {
    auto La = a.normalize();
    auto Lb = b.normalize();
    auto Lt = La + (Lb-La)*t;
    auto cp = a ^ b;
    auto R = asinf(cp);
    auto r = R * t;
    auto p = sinf(R-r) / cp;
    auto q = sinf(r) / cp;
    return (a*p + b*q) * Lt;
  }
};


template<>
inline Vec2 Vec2::unit<0> () noexcept {
  return {1.0F, 0.0F};
}
template<>
inline Vec2 Vec2::unit<1> () noexcept {
  return {0.0F, 1.0F};
}


inline Vec2 operator* (f32 a, Vec2 b) noexcept {
  return b * a;
}


CLOSE_JLIB_NS
